package edu.wpi.first.wpilibj.swarm;
import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.Utility;

public class ManualGyro implements Runnable {
	AnalogChannel input;
	double        accum_calibration;
	long          ticks_calibration;
	double        avg_volts;
	double        volts_per_dps;
	
	public ManualGyro(int analoginput) {
		input     = new AnalogChannel(analoginput);
		input.setAverageBits(10);
		avg_volts = 2.5;
		volts_per_dps = .007;
		accum_calibration = 0.0;
		ticks_calibration = 0;
		init();
	}

	void runCalibration() {
		accum_calibration += input.getAverageVoltage();
		ticks_calibration++;
	}
	
	void finishCalibration() {
		avg_volts = accum_calibration / ticks_calibration;
	}

	private void init() {
		new Thread(this).start();
	}
	
	double getRate() {
		return (input.getAverageVoltage() - avg_volts)/volts_per_dps;
	}

	public void run() {
		long tgttime = Utility.getFPGATime() + 1000000 * 5;
		while (Utility.getFPGATime() < tgttime) {
			try {
				Thread.sleep(1);
			} catch (InterruptedException ex) {}
			runCalibration();
		}
		finishCalibration();
	}
}